#!/usr/bin/python
# -*- coding: UTF-8 -*-

# 图像处理相关功能包
from picamera.array import PiRGBArray
from picamera import PiCamera
import time
import cv2
import numpy as np
# 机器人控制相关功能表
from robotPi import robotPi
# c-lib 相关功能包
import ctypes
lib_vp = ctypes.CDLL("/home/pi/code/metis/lib/metis_vp.so")

# 图像相关参数
width = 480
height = 180
resized_height = 300

def initPiCamera():
    # 1. 初始化相机
    camera = PiCamera()
    # 2. 设计相机视频流的分辨率
    camera.resolution = (640, 480)
    # 3. 设置相机视频里的帧率
    camera.framerate = 30
    # 4. 画面翻转设置
    camera.hflip =False
    camera.vflip = False
    # 5. 设置相机快门速度(us)
    camera.shutter_speed = 200000 
    # 6. 设置相机感光度（ISO）
    camera.iso = 100
    # allow the camera to warmup
    time.sleep(0.1)
    return camera


def metis_trace(camera):
# 循迹主程序
    # 实例化机器人控制对象
    robot = robotPi()
    # 初始化c变量对象
    v_turn = ctypes.c_int(0)
    v_run = ctypes.c_int(0)
    eventFlag = ctypes.c_int(0)
    # 初始化相机原始画面捕获
    rawCapture = PiRGBArray(camera, size=(640, 480))   
    # 记录起始时间戳
    timeStart = cv2.getTickCount()
    
    for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
        # ————视频流主循环—————————
        # 1. 得到原始画面的像素矩阵
        imageRaw = frame.array 
        # 2. 重新设置图像的大小
        imageResized = cv2.resize(imageRaw, (480, resized_height)) # 设置宽度从640->480
        imageResized = imageResized[resized_height - height:, :] # 设置画面高度
        # 3. 只保留图像的黑白通道
        imageGray = cv2.cvtColor(imageResized, cv2.COLOR_BGR2GRAY)   
        ret,imageThreshold = cv2.threshold(imageGray,30,255,cv2.THRESH_BINARY)      
        cv2.imshow("image",imageThreshold) # 显示图像
        # 4. 转化为numpy矩阵以便输入c程序
        imageArray = np.array(imageGray, dtype=np.float32).astype(int)
        # 5. 获取当前时间戳
        timeStamp = int((cv2.getTickCount() - timeStart)/ cv2.getTickFrequency() * 1000)
        # 6. 将像素矩阵输入c-lib进行主控制与决策，最终实现机器人控制
        lib_vp.control(imageArray.ctypes.data_as(ctypes.POINTER(ctypes.c_int)),
                       ctypes.byref(v_turn),
                       ctypes.byref(v_run),
                       ctypes.byref(eventFlag),
                       ctypes.c_int(timeStamp))
        print('[',timeStamp,']vt=',v_turn.value,'vr=',v_run.value,'flag=',eventFlag.value)
        # 控制机器人
        robot.movement.move_free(0, v_run.value, v_turn.value, 200)
        # 7. 击靶指令判别
        if(timeStamp>6000):
            if(eventFlag.value>13):
                print('___hit___')
                robot.movement.move_right(40,300)
                time.sleep(0.30)
                robot.movement.hit()
                break
        # 8. 超时判别
        if(timeStamp>20000):
            break
        # 清空视频流，接收下一帧
        rawCapture.truncate(0)
        q = cv2.waitKey(1)
    cv2.destroyAllWindows()
   
    
if __name__ == '__main__':
    # 获得相机对象
    c = initPiCamera()
    # 自动寻路
    metis_trace(c)
    exit(0)